/////////////////////////////////////////////////////////////////////////
//
// Amuse Engine SDK - core/math
// Copyright (c) 2014.  All Rights Reserved
//
// File:		AEVector4f.h
// Author:		Gianluca Belardelli
// Date:		29/07/2013
// Purpose:		Implementazione dei metodi inline della classe AEVector4f
//
// Revision:
//				001 - 29/07/2013
//					+ Revisione iniziale
//
/////////////////////////////////////////////////////////////////////////
#ifndef _AEVECTOR4_INL_
#define _AEVECTOR4_INL_

AE_FORCEINLINE AEVector4f::AEVector4f( void )
{
}

AE_FORCEINLINE const AEVector4f AEVector4f::ZeroVector( void )
{
	return AEVector4f( g_vectorfConstants[AE_QUADREAL_0] );
}

template<AEINT32 vectorConstant>
AE_FORCEINLINE const AEVector4f& AE_CALL AEVector4f::GetConstant( void )
{
	return *(const AEVector4f*) (g_vectorfConstants + vectorConstant);
}

AE_FORCEINLINE const AEVector4f& AE_CALL AEVector4f::GetConstant( AEVectorConstant vcConstant )
{
	return *(const AEVector4f*) (g_vectorfConstants + vcConstant);
}

AE_FORCEINLINE void AEVector4f::Add( AEVector4fRefParam vcVector )
{
	SetAdd( *this, vcVector );
}

AE_FORCEINLINE void AEVector4f::Sub( AEVector4fRefParam vcVector )
{
	SetSub( *this, vcVector );
}

AE_FORCEINLINE void AEVector4f::Mul( AEVector4fRefParam vcVector )
{
	SetMul( *this, vcVector );
}

AE_FORCEINLINE void AEVector4f::Mul( AESimdFloat32ConstRef fValue )
{              
	SetMul( *this, fValue );
}

AE_FORCEINLINE void AEVector4f::AddMul( AEVector4fRefParam vcVector, AESimdFloat32ConstRef fValue )
{
	SetAddMul(*this, vcVector, fValue );
}

AE_FORCEINLINE void AEVector4f::AddMul( AESimdFloat32ConstRef fValue, AEVector4fRefParam vcVector )
{
	SetAddMul(*this, vcVector, fValue );
}

AE_FORCEINLINE void AEVector4f::SubMul( AEVector4fRefParam vcVector, AESimdFloat32ConstRef fValue )
{
	SetSubMul(*this, vcVector, fValue );
}

AE_FORCEINLINE void AEVector4f::SetMul( AESimdFloat32ConstRef fValue, AEVector4fRefParam vcVector )
{
	SetMul( vcVector, fValue );
}

AE_FORCEINLINE void AEVector4f::AddMul( AEVector4fRefParam vcVector1, AEVector4fRefParam vcVector2 )
{
	SetAddMul( *this, vcVector1, vcVector2 );
}

AE_FORCEINLINE void AEVector4f::SubMul( AEVector4fRefParam vcVector1, AEVector4fRefParam vcVector2 )
{
	SetSubMul(*this, vcVector1, vcVector2 );
}

AE_FORCEINLINE void AEVector4f::SubMul( AESimdFloat32ConstRef fValue, AEVector4fRefParam vcVector )
{
	SetSubMul(*this, vcVector, fValue );
}

template <AEINT32 N> 
AE_FORCEINLINE void AEVector4f::SetHorizontalAdd( AEVector4fRefParam vcVector )
{
	SetAll( vcVector.GetHorizontalAdd<N>() );
}

template <AEINT32 N> 
AE_FORCEINLINE void AEVector4f::SetHorizontalMax( AEVector4fRefParam vcVector )
{
	SetAll( vcVector.GetHorizontalMax<N>() );
}

template <AEINT32 N> 
AE_FORCEINLINE void AEVector4f::SetHorizontalMin( AEVector4fRefParam vcVector )
{
	SetAll( vcVector.GetHorizontalMin<N>() );
}

template <AEINT32 N>
AE_FORCEINLINE void AEVector4f::SetDot( AEVector4fRefParam vcVector1, AEVector4fRefParam vcVector2 )
{
	SetAll( vcVector1.GetDot<N>( vcVector2 ) );
}

template <AEINT32 N, AEMathAccuracyMode A, AEMathNegSqrtMode S> 
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLength( void ) const
{
	const AESimdFloat32 len2 = GetLengthSquared<N>();
	return len2.Sqrt<A,S>();
}

template <AEINT32 N>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLength( void ) const
{
	return GetLength<N, AE_ACC_23_BIT, AE_SQRT_SET_ZERO>();
}

template <AEINT32 N>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLengthSquared( void ) const
{
	return GetDot<N>(*this);
}

template <AEINT32 N, AEMathAccuracyMode A, AEMathNegSqrtMode S> 
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLengthInverse( void ) const
{
	const AESimdFloat32 len2 = GetLengthSquared<N>();
	return len2.SqrtInverse<A,S>();
}

template <AEINT32 N>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLengthInverse( void ) const
{
	return GetLengthInverse<N, AE_ACC_23_BIT, AE_SQRT_SET_ZERO>();
}

template <AEINT32 N, AEMathAccuracyMode A, AEMathNegSqrtMode S>
AE_FORCEINLINE void AEVector4f::Normalize( void )
{
	Mul( GetLengthInverse< N, A, S >() );
}

template <AEINT32 N>
AE_FORCEINLINE void AEVector4f::Normalize( void )
{
	Normalize<N, AE_ACC_23_BIT, AE_SQRT_SET_ZERO>();
}

template <AEINT32 N, AEMathAccuracyMode A, AEMathNegSqrtMode S>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLengthAndNormalize( void )
{
	const AESimdFloat32 len2 = GetLengthSquared<N>();
	const AESimdFloat32 lenInv = len2.SqrtInverse<A,S>();
	Mul(lenInv);
	return (len2 * lenInv);
}

template <AEINT32 N>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetLengthAndNormalize( void )
{
	return GetLengthAndNormalize<N, AE_ACC_23_BIT, AE_SQRT_SET_ZERO>();
}

template <AEMathAccuracyMode A, AEMathNegSqrtMode S>
AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetDistanceTo( AEVector4fRefParam vcVector ) const
{
	const AESimdFloat32 d2 = GetDistanceToSquared( vcVector );
	const AESimdFloat32 ri = d2.SqrtInverse<A,S>();
	return (d2 * ri);
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetDistanceTo( AEVector4fRefParam vcVector ) const
{
	return GetDistanceToSquared( vcVector ).Sqrt<AE_ACC_23_BIT, AE_SQRT_SET_ZERO>();
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetDistanceToSquared( AEVector4fRefParam vcVector ) const
{
	AEVector4f d;
	d.SetSub( *this, vcVector );
	return d.GetLengthSquared<3>();
}

AE_FORCEINLINE void AEVector4f::SetW( AESimdFloat32ConstRef fW )
{
	SetComponent<3>( fW );
}

AE_FORCEINLINE void AEVector4f::operator= ( AEVector4fRefParam vcVector )
{
	m_qfVector = vcVector.m_qfVector;
}

AE_FORCEINLINE void AEVector4f::SetPlaneConstant( AEVector4fRefParam vcPointOnPlane )
{
	const AESimdFloat32 fW = GetDot<3>(vcPointOnPlane);
	SetW( -fW );
}

AE_FORCEINLINE void AEVector4f::SetClamped( const AEVector4fRefParam vcVal, const AEVector4fRefParam vcMinVal, const AEVector4fRefParam vcMaxVal )
{
	// This ensures that if a is NAN, clamped will be maxVal afterwards	
	const AEVector4fComparison maxValGtA = vcMaxVal.CmpG( vcVal );
	AEVector4f clamped;
	clamped.SetSelect( maxValGtA, vcVal, vcMaxVal);
	SetMax( vcMinVal, clamped );
}

AE_FORCEINLINE void AEVector4f::SetInterpolate( AEVector4fRefParam vcVector1, AEVector4fRefParam vcVector2, AESimdFloat32ConstRef fValue )
{
	// vcVector1 + fValue * (vcVector2 - vcVector1)
	AEVector4f d;
	d.SetSub(vcVector2, vcVector1);
	SetAddMul( vcVector1, d, fValue );
}

template <AEINT32 N> 
AE_FORCEINLINE bool AEVector4f::IsNormalized( AEFLOAT32 fEpsilon ) const
{
	if( IsOk<N>() )
	{
		const AESimdFloat32 lenZero = GetLengthSquared<N>() - AESimdFloat32::GetConstant<AE_QUADREAL_1>();
		AESimdFloat32 absLenZero;
		absLenZero.SetAbs( lenZero );
		return absLenZero < AESimdFloat32::FromFloat( fEpsilon );
	}

	return false;
}

template <AEINT32 N, AEMathAccuracyMode A, AEMathNegSqrtMode S>
AE_FORCEINLINE AEBOOL32 AEVector4f::NormalizeIfNotZero( void )
{
	const AESimdFloat32 lengthSqrd = GetLengthSquared<N>();
#if (AE_CONFIG_SIMD == AE_CONFIG_SIMD_ENABLED)
	const AEVector4fComparison less0 = ( -lengthSqrd ).CmpLessZero();

	AEVector4f normThis;
	
	normThis.SetMul( *this, lengthSqrd.SqrtInverse<A,S>() );
	SetSelect( less0, normThis, *this );

	return less0.AnyIsSet();
#else
	if ( ( -lengthSqrd ).IsLessZero() )
	{
		mul( lengthSqrd.SqrtInverse<A,S>() );
		return 1;
	}
	
	return 0;
#endif
}

template <AEINT32 N>
AE_FORCEINLINE AEBOOL32 AEVector4f::NormalizeIfNotZero( void )
{
	return NormalizeIfNotZero<N,AE_ACC_23_BIT,AE_SQRT_SET_ZERO>();
}

template<> 
AE_FORCEINLINE void AEVector4f::SetSelect<AEVector4ComparisonMask::MASK_NONE>( AEVector4fRefParam vcTrueValue, AEVector4fRefParam vcFalseValue )
{
	*this = vcFalseValue;
}

template<> 
AE_FORCEINLINE void AEVector4f::SetSelect<AEVector4ComparisonMask::MASK_XYZW>( AEVector4fRefParam vcTrueValue, AEVector4fRefParam vcFalseValue )
{
	*this = vcTrueValue;
}

AE_FORCEINLINE AEBOOL32 operator== ( const AEVector4f &vcLhs, const AEVector4f &vcRhs )
{
	return vcLhs.CmpE( vcRhs ).AllAreSet();
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetX( void ) const
{
	return GetComponent<0>();
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetY( void ) const
{
	return GetComponent<1>();
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetZ( void ) const
{
	return GetComponent<2>();
}

AE_FORCEINLINE const AESimdFloat32 AEVector4f::GetW( void ) const
{
	return GetComponent<3>();
}

#endif // _AEVECTOR4_INL_
